
#include "HeaderFiles.h"

MT_SPDCAL MT_calculate[2];


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void timer5_cnt_init(void)
{
    timer_parameter_struct timer_initpara;
	
	rcu_periph_clock_enable(RCU_TIMER5);
	timer_deinit(TIMER5);
	
	// timer 5 overtime set 64ms.
	timer_initpara.prescaler         = 128-1;
	timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
	timer_initpara.period            = MT_CAL_TIME_CNT_MAX-1;
	timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(TIMER5,&timer_initpara);
	
	nvic_irq_enable(TIMER5_IRQn, 0, 0);
	timer_interrupt_enable(TIMER5, TIMER_INT_UP);
	
    timer_auto_reload_shadow_enable(TIMER5);
	
	#if 1
    timer_enable(TIMER5);
	#else
	timer_disable(TIMER5);
	#endif
	
	timer_counter_value_config(TIMER5, 0);
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void timer4_cnt_init(void)
{
    timer_parameter_struct timer_initpara;
	
	rcu_periph_clock_enable(RCU_TIMER4);
	timer_deinit(TIMER4);
	
	// timer 4 overtime set 64ms.
	timer_initpara.prescaler         = 128-1;
	timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
	timer_initpara.counterdirection  = TIMER_COUNTER_UP;
	timer_initpara.period            = MT_CAL_TIME_CNT_MAX-1;
	timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
	timer_initpara.repetitioncounter = 0;
	timer_init(TIMER4,&timer_initpara);
	
	nvic_irq_enable(TIMER4_IRQn, 0, 0);
	timer_interrupt_enable(TIMER4, TIMER_INT_UP);
	
    timer_auto_reload_shadow_enable(TIMER4);
	
	#if 1
    timer_enable(TIMER4);
	#else
	timer_disable(TIMER4);
	#endif
	
	timer_counter_value_config(TIMER4, 0);
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void MTspeedcal_init(void)
{
	// motor #1 encoder GPIO input PA0
	// motor #2 encoder GPIO input PD12

	nvic_irq_enable(EXTI0_IRQn, 0U, 0U);  
	gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOA, GPIO_PIN_SOURCE_0);
	exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_FALLING); 
	exti_interrupt_flag_clear(EXTI_0);  
	
	nvic_irq_enable(EXTI10_15_IRQn, 0U, 0U);  
	gpio_exti_source_select(GPIO_PORT_SOURCE_GPIOD, GPIO_PIN_SOURCE_12);
	exti_init(EXTI_12, EXTI_INTERRUPT, EXTI_TRIG_RISING); 
	exti_interrupt_flag_clear(EXTI_12);  
	
	nvic_irq_disable(EXTI0_IRQn);
	nvic_irq_disable(EXTI10_15_IRQn);
	
	// timer5 user for motor right speed cnt
	timer5_cnt_init();
	
	// timer4 user for motor left speed cnt
	timer4_cnt_init();
	
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void MTspeedcal_start(void)
{
	nvic_irq_enable(EXTI0_IRQn, 0U, 0U);
	exti_interrupt_flag_clear(EXTI_0);  

	nvic_irq_enable(EXTI10_15_IRQn, 0U, 0U);
	exti_interrupt_flag_clear(EXTI_12);  
	
	MT_calculate[0].ISR_enable = 1;
	MT_calculate[1].ISR_enable = 1;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void TIMER5_IRQHandler(void)
{

	timer_flag_clear(TIMER5,TIMER_FLAG_UP);
	
	MT_calculate[1].speed_rps = 0;
	
	timer_counter_value_config(TIMER5, 0);
	timer_disable(TIMER5);
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void TIMER4_IRQHandler(void)
{

	timer_flag_clear(TIMER4,TIMER_FLAG_UP);
	
	MT_calculate[0].speed_rps = 0;
	
	timer_counter_value_config(TIMER4, 0);
	timer_disable(TIMER4);
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void MT_speed_calculate(MT_SPDCAL *MThdl, MT_MOTOR_NUM num)
{
	
	int16_t eqep_erro = 0;

	if(MThdl->ISR_enable == 1)
	{
		MThdl->ISR_enable = 0;
				
		if(num == MT_MOTOR_LEFT)
		{
			MThdl->timer_cnt_now = timer_counter_read(TIMER4);
			MThdl->eqep_cnt_now  = timer_counter_read(TIMER3);
			
			timer_counter_value_config(TIMER4, 0);
		}
		else
		{
			MThdl->timer_cnt_now = timer_counter_read(TIMER5);
			MThdl->eqep_cnt_now  = timer_counter_read(TIMER1);
			
			timer_counter_value_config(TIMER5, 0);
		}
		
		eqep_erro = MThdl->eqep_cnt_now - MThdl->eqep_cnt_old;
		
		if(eqep_erro > MAGNET_ENCODER_LINE_NUM_HALF)
		{
			eqep_erro = eqep_erro - MAGNET_ENCODER_LINE_NUM;
		}
		
		else if(eqep_erro < -MAGNET_ENCODER_LINE_NUM_HALF)
		{
			eqep_erro = eqep_erro + MAGNET_ENCODER_LINE_NUM;
		}
		
		MThdl->eqep_cnt_diff = eqep_erro;
		
		if(MThdl->timer_cnt_now != 0)
		{
			MThdl->timer_sf = MThdl->timer_cnt_now * MT_CAL_TIME_SF;
			
			MThdl->speed_rps = ((float32_t)MThdl->eqep_cnt_diff / MThdl->timer_sf) * MT_CAL_SPEED_SF;
			
			MThdl->speed_update = 1;
		}
		else
		{
			MThdl->speed_rps = 0;
		}		
		
		MThdl->timer_cnt_old = MThdl->timer_cnt_now;
		MThdl->eqep_cnt_old  = MThdl->eqep_cnt_now;
	}
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void EXTI0_IRQHandler(void)
{	
		
	exti_interrupt_flag_clear(EXTI_0); 
	
	#if 0
	nvic_irq_disable(EXTI0_IRQn);
	#endif
		
	MT_speed_calculate(&MT_calculate[1], MT_MOTOR_RIGHT);
	
	timer_enable(TIMER5);
}

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void EXTI10_15_IRQHandler(void)
{	
		
	exti_interrupt_flag_clear(EXTI_12); 
	
	#if 0
	nvic_irq_disable(EXTI0_IRQn);
	#endif
		
	MT_speed_calculate(&MT_calculate[0], MT_MOTOR_LEFT);
	
	timer_enable(TIMER4);
}




